from collections import deque

def find_path_BFS(start, finish, cells, bounds):
	"""
	Very ugly BFS implementation. TODO: Maybe store depth in cells to avoid usage of bunch of helper containers.
	returns list of nodes if path is found, None otherwise
	"""

	print('in find_path_BFS')
	if start == finish:
		return [finish]

	depth = 0

	queue = deque()
	depth_queue = deque()

	visited = {}
	visited_by_depth = {}

	queue.append(start)
	depth_queue.append(depth)

	while len(queue) > 0:
		print('queue: {0}'.format(queue))
		
		item = queue.popleft()
		item_depth = depth_queue.popleft()

		depth = item_depth + 1
		
		visited[item] = item_depth
		if item_depth in visited_by_depth:
			visited_by_depth[item_depth].append(item)
		else:
			visited_by_depth[item_depth] = [item]
			
		if item == finish:
			print('find_path_BFS success. Start: {0} Finish: {1} Depth: {2}'.format(start, finish, depth))
			print(visited_by_depth)
			path = []
			path.append(item)
			#print("Depth of finish item: {0} {1}".format(item_depth, visited[item]))
			for d in range(visited[item] - 1, -1, -1):
				print('d: {0} list: {1}'.format(d, visited_by_depth.get(d)))
				l = visited_by_depth.get(d)
				path.append(l[0])
			path.reverse()
			print("Path: {0}".format(path))
			assert(path[0] == start)
			return path
		
		if item[0] > 0:
			left = (item[0] - 1, item[1])
			if cells[left[0]][left[1]].is_free() is True and left not in visited and left not in queue:
				assert(left not in queue)
				queue.append(left)
				depth_queue.append(depth)

		if item[0] < bounds[0] - 1:
			right = (item[0] + 1, item[1])
			if cells[right[0]][right[1]].is_free() is True and right not in visited and right not in queue:
				assert(right not in queue)
				queue.append(right)
				depth_queue.append(depth)

		if item[1] > 0:
			top = (item[0], item[1] - 1)
			if cells[top[0]][top[1]].is_free() is True and top not in visited and top not in queue:
				assert(top not in queue)
				queue.append(top)
				depth_queue.append(depth)

		if item[1] < bounds[1] - 1:
			bottom = (item[0], item[1] + 1)
			if cells[bottom[0]][bottom[1]].is_free() is True and bottom not in visited and bottom not in queue:
				assert(bottom not in queue)
				queue.append(bottom)
				depth_queue.append(depth)

		print("BFS depth {0}".format(depth))

	print('out find_path_BFS')
	return None

